//------------------------------------------------------------------------------
//  <copyright file="KukaLBR4OperationsTest.h" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
//  </copyright>
//------------------------------------------------------------------------------

#include "stdafx.h"
#include "KukaLBR4OperationsTest.h"

#include <KukaLBR4Operations.h>

#define _USE_MATH_DEFINES
#include <math.h>
#include <sstream>

#define ERROR_EPSILON 0.0001

void KukaLBR4OperationsTest::TestCeeOpAdd()
{
	// srand cannot take a value larger than an unsigned int, so no reason to use GetTickCount64()
#pragma warning(suppress: 28159)
	srand(GetTickCount());

	double pOperand1[1] = {KukaLBR4OperationsTest::GetRandomNumber()};
	double pOperand2[1] = {KukaLBR4OperationsTest::GetRandomNumber()};
	double pResult[1];

	double *ppParameters[3] = {
		pResult,
		pOperand1,
		pOperand2 };

	KukaLBR4OperationsNS::CeeOpAdd(ppParameters);

	KUKALBR4NATIVEASSERT(pResult[0] == pOperand1[0] + pOperand2[0], "Result is equal to sum of first two operands");
}

/// <summary> Test the <c>CeeOpAddVec6</c> operation </summary>
void KukaLBR4OperationsTest::TestCeeOpAddVec6()
{
	// srand cannot take a value larger than an unsigned int, so no reason to use GetTickCount64()
#pragma warning(suppress: 28159)
	srand(GetTickCount());

	double pOperand1[6] = {
		KukaLBR4OperationsTest::GetRandomNumber(),
		KukaLBR4OperationsTest::GetRandomNumber(),
		KukaLBR4OperationsTest::GetRandomNumber(),
		KukaLBR4OperationsTest::GetRandomNumber(),
		KukaLBR4OperationsTest::GetRandomNumber(),
		KukaLBR4OperationsTest::GetRandomNumber() };

	double pOperand2[6] = {
		KukaLBR4OperationsTest::GetRandomNumber(),
		KukaLBR4OperationsTest::GetRandomNumber(),
		KukaLBR4OperationsTest::GetRandomNumber(),
		KukaLBR4OperationsTest::GetRandomNumber(),
		KukaLBR4OperationsTest::GetRandomNumber(),
		KukaLBR4OperationsTest::GetRandomNumber() };

	double pResult[6];

	double *ppParameters[3] = {
		pResult,
		pOperand1,
		pOperand2 };

	KukaLBR4OperationsNS::CeeOpAddVec6(ppParameters);

	for (int i = 0; i < 6; ++i)
	{
		KUKALBR4NATIVEASSERT(pResult[i] == pOperand1[i] + pOperand2[i], "Each element in result is equal to sum of corresponding elements in operands");
	}
}

/// <summary> Test the <c>CeeOpMultiply</c> operation </summary>
void KukaLBR4OperationsTest::TestCeeOpMultiply()
{
	// srand cannot take a value larger than an unsigned int, so no reason to use GetTickCount64()
#pragma warning(suppress: 28159)
	srand(GetTickCount());

	double pOperand1[1] = {KukaLBR4OperationsTest::GetRandomNumber()};
	double pOperand2[1] = {KukaLBR4OperationsTest::GetRandomNumber()};
	double pResult[1];

	double *ppParameters[3] = {
		pResult,
		pOperand1,
		pOperand2 };

	KukaLBR4OperationsNS::CeeOpMultiply(ppParameters);

	KUKALBR4NATIVEASSERT(ppParameters[0][0] == pOperand1[0] * pOperand2[0], "Result is equal to product of first two operands");
}

/// <summary> Test the <c>CeeOpIsMagnitudeGreaterThanVec6</c> operation </summary>
void KukaLBR4OperationsTest::TestCeeOpIsMagnitudeGreaterThanVec6()
{
	// srand cannot take a value larger than an unsigned int, so no reason to use GetTickCount64()
#pragma warning(suppress: 28159)
	srand(GetTickCount());

	double pOperand1[6] = {
		KukaLBR4OperationsTest::GetRandomNumber(),
		KukaLBR4OperationsTest::GetRandomNumber(),
		KukaLBR4OperationsTest::GetRandomNumber(),
		KukaLBR4OperationsTest::GetRandomNumber(),
		KukaLBR4OperationsTest::GetRandomNumber(),
		KukaLBR4OperationsTest::GetRandomNumber() };

	double pOperand2[1] = {
		KukaLBR4OperationsTest::GetRandomNumber() };

	double pResult[1];

	double sumOfSquares = 0;

	for(int i = 0; i < 6; ++i)
	{
		sumOfSquares += pOperand1[i] * pOperand1[i];
	}

	double magnitude = sqrt(sumOfSquares);

	double expectedResult = magnitude > pOperand2[0] ? 1 : 0;

	double *ppParameters[3] = {
		pResult,
		pOperand1,
		pOperand2 };

	KukaLBR4OperationsNS::CeeOpIsMagnitudeGreaterThanVec6(ppParameters);

	KUKALBR4NATIVEASSERT(pResult[0] == expectedResult, "Result of CeeOpIsMagnitudeGreaterThanVec6 is expected value.");
}

/// <summary> Test the <c>CeeOpIsAnyElementGreaterThanVec6</c> operation </summary>
void KukaLBR4OperationsTest::TestCeeOpIsAnyElementGreaterThanVec6()
{
	// srand cannot take a value larger than an unsigned int, so no reason to use GetTickCount64()
#pragma warning(suppress: 28159)
	srand(GetTickCount());

	double pOperand1[6] = {
		KukaLBR4OperationsTest::GetRandomNumber(),
		KukaLBR4OperationsTest::GetRandomNumber(),
		KukaLBR4OperationsTest::GetRandomNumber(),
		KukaLBR4OperationsTest::GetRandomNumber(),
		KukaLBR4OperationsTest::GetRandomNumber(),
		KukaLBR4OperationsTest::GetRandomNumber() };

	double pOperand2[6] = {
		KukaLBR4OperationsTest::GetRandomNumber(),
		KukaLBR4OperationsTest::GetRandomNumber(),
		KukaLBR4OperationsTest::GetRandomNumber(),
		KukaLBR4OperationsTest::GetRandomNumber(),
		KukaLBR4OperationsTest::GetRandomNumber(),
		KukaLBR4OperationsTest::GetRandomNumber() };

	double pResult[6];

	double expectedResult = 0;
	for(int i = 0; i < 6; i++)
	{
		if(pOperand1[i] > pOperand2[i])
		{
			expectedResult = 1;
			break;
		}
	}

	double *ppParameters[3] = {
		pResult,
		pOperand1,
		pOperand2 };

	KukaLBR4OperationsNS::CeeOpIsAnyElementGreaterThanVec6(ppParameters);

	KUKALBR4NATIVEASSERT(pResult[0] == expectedResult, "Result of CeeOpIsAnyElementGreaterThanVec6 is expected value.");
}

/// <summary> Test the <c>CeeOpCopyVec7</c> operation </summary>
void KukaLBR4OperationsTest::TestCeeOpCopyVec7()
{
	// srand cannot take a value larger than an unsigned int, so no reason to use GetTickCount64()
#pragma warning(suppress: 28159)
	srand(GetTickCount());

	double pOperand1[7] = {
		KukaLBR4OperationsTest::GetRandomNumber(),
		KukaLBR4OperationsTest::GetRandomNumber(),
		KukaLBR4OperationsTest::GetRandomNumber(),
		KukaLBR4OperationsTest::GetRandomNumber(),
		KukaLBR4OperationsTest::GetRandomNumber(),
		KukaLBR4OperationsTest::GetRandomNumber(),
		KukaLBR4OperationsTest::GetRandomNumber() };

	double pResult[7] = {
		KukaLBR4OperationsTest::GetRandomNumber(),
		KukaLBR4OperationsTest::GetRandomNumber(),
		KukaLBR4OperationsTest::GetRandomNumber(),
		KukaLBR4OperationsTest::GetRandomNumber(),
		KukaLBR4OperationsTest::GetRandomNumber(),
		KukaLBR4OperationsTest::GetRandomNumber(),
		KukaLBR4OperationsTest::GetRandomNumber() };

	double *ppParameters[2] = {
		pResult,
		pOperand1 };

	KukaLBR4OperationsNS::CeeOpCopyVec7(ppParameters);

	for (int i = 0; i < 7; ++i)
	{
		KUKALBR4NATIVEASSERT(pResult[i] == pOperand1[i], "Each element in result is equal to corresponding element in operand");
	}
}

/// <summary> Test the <c>CeeOpCopyVec6</c> operation </summary>
void KukaLBR4OperationsTest::TestCeeOpCopyVec6()
{
	// srand cannot take a value larger than an unsigned int, so no reason to use GetTickCount64()
#pragma warning(suppress: 28159)
	srand(GetTickCount());

	double pOperand1[6] = {
		KukaLBR4OperationsTest::GetRandomNumber(),
		KukaLBR4OperationsTest::GetRandomNumber(),
		KukaLBR4OperationsTest::GetRandomNumber(),
		KukaLBR4OperationsTest::GetRandomNumber(),
		KukaLBR4OperationsTest::GetRandomNumber(),
		KukaLBR4OperationsTest::GetRandomNumber() };

	double pResult[6] = {
		KukaLBR4OperationsTest::GetRandomNumber(),
		KukaLBR4OperationsTest::GetRandomNumber(),
		KukaLBR4OperationsTest::GetRandomNumber(),
		KukaLBR4OperationsTest::GetRandomNumber(),
		KukaLBR4OperationsTest::GetRandomNumber(),
		KukaLBR4OperationsTest::GetRandomNumber() };

	double *ppParameters[2] = {
		pResult,
		pOperand1 };

	KukaLBR4OperationsNS::CeeOpCopyVec6(ppParameters);

	for (int i = 0; i < 6; ++i)
	{
		KUKALBR4NATIVEASSERT(pResult[i] == pOperand1[i], "Each element in result is equal to corresponding element in operand");
	}
}

/// <summary> Test the <c>CeeOpSimpleJointInterpolation</c> operation </summary>
void KukaLBR4OperationsTest::TestCeeOpSimpleJointInterpolation()
{
	double pCurrentPosition[7] = {
		10,
		20,
		30,
		40,
		50,
		60,
		70 };

	double pDesiredPosition[7] = {
		100,
		100,
		100,
		100,
		100,
		100,
		100 };

	double pMaxVelocity[1] = { 9 };

	double pCycleTime[1] = { 1000 };

	double pDurationInCycles[1] = { 1 };

	double pCurrentCycleNumber[1] = { 1 };

	double pResult[7];

	double *ppParameters[7] = {
		pResult,
		pCurrentPosition,
		pDesiredPosition,
		pMaxVelocity,
		pCycleTime,
		pDurationInCycles,
		pCurrentCycleNumber};

	KukaLBR4OperationsNS::CeeOpSimpleJointInterpolation(ppParameters);

	for (int i = 0; i < 7; ++i)
	{
		KUKALBR4NATIVEASSERT(pResult[i] == pCurrentPosition[i] + ((pDesiredPosition[i] - pCurrentPosition[i])/10.0), "Each element in result is the correct next position command.");
	}
}

/// <summary> Test the <c>CeeOpSimpleJointPositionInterpolation</c> operation </summary>
void KukaLBR4OperationsTest::TestCeeOpSimpleCartesianInterpolation()
{
	double pCurrentPosition[CARTESIAN_FRAME_LENGTH] = {
		1, 0, 0, 0,
		0, 1, 0, 0,
		0, 0, 1, 0};

	double pDesiredPosition[CARTESIAN_FRAME_LENGTH] = {
		0, 0, 1, 10,
		1, 0, 0, 20,
		0, 1, 0, 30};

	double pMaxLinearVelocity[1] = { 10 };

	double pMaxRotationalVelocity[1] = { 3 };

	double pCycleTime[1] = { 1000 };

	double pResult[CARTESIAN_FRAME_LENGTH];

	double *ppParameters[6] = {
		pResult,
		pCurrentPosition,
		pDesiredPosition,
		pMaxLinearVelocity,
		pMaxRotationalVelocity,
		pCycleTime};

	// This should complete in 3 cycles
	for(int i = 0; i<3; ++i)
	{
		KukaLBR4OperationsNS::CeeOpSimpleCartesianInterpolation(ppParameters);
		for(int j = 0; j< CARTESIAN_FRAME_LENGTH; ++j)
		{
			pCurrentPosition[j] = pResult[j];
		}
	}

	for (int i = 0; i < CARTESIAN_FRAME_LENGTH; ++i)
	{
		KUKALBR4NATIVEASSERT((abs(pResult[i] - pDesiredPosition[i]) < ERROR_EPSILON), "Each element in result is the correct next position command.");
	}
}

/// <summary> Test the <c>CeeOpSimpleJointPositionInterpolation</c> operation using real data. </summary>
void KukaLBR4OperationsTest::TestCeeOpSimpleCartesianInterpolation2()
{
	double pCurrentPosition[CARTESIAN_FRAME_LENGTH] = 
	{
		-0.020687149837613106,
		-0.056298475712537766,
		-0.99819964170455933,
		-0.13593971729278564,
		0.38612651824951172,
		0.92049777507781982,
		-0.059918351471424103,
		0.40853497385978699,
		0.92221385240554810,
		-0.38667088747024536,
		0.0026958598755300045,
		0.14615613222122192
	};

	double pDesiredPosition[CARTESIAN_FRAME_LENGTH] = 
	{
		0.22690424323081970,
		0.97315621376037598,
		-0.038489356637001038,
		-0.093268766999244690,
		0.059803504496812820,
		0.025523381307721138,
		0.99788379669189453,
		0.82020062208175659,
		0.97207921743392944,
		-0.22872586548328400,
		-0.052406787872314453,
		0.13197985291481018
	};

	double pMaxLinearVelocity[1] = { 10 };

	double pMaxRotationalVelocity[1] = { 0.1 };

	double pCycleTime[1] = { 20 };

	double pResult[CARTESIAN_FRAME_LENGTH];

	double *ppParameters[6] = {
		pResult,
		pCurrentPosition,
		pDesiredPosition,
		pMaxLinearVelocity,
		pMaxRotationalVelocity,
		pCycleTime};

	for(int i = 0; i<1000; ++i)
	{
		KukaLBR4OperationsNS::CeeOpSimpleCartesianInterpolation(ppParameters);
		for(int j = 0; j< CARTESIAN_FRAME_LENGTH; ++j)
		{
			pCurrentPosition[j] = pResult[j];
		}
	}

	for (int i = 0; i < CARTESIAN_FRAME_LENGTH; ++i)
	{
		KUKALBR4NATIVEASSERT((abs(pResult[i] - pDesiredPosition[i]) < ERROR_EPSILON), "Each element in result is the correct next position command.");
	}
}

/// <summary> Test the <c>SimpleInterpolation</c> function. </summary>
void KukaLBR4OperationsTest::TestSimpleInterpolation()
{
	const double initialPos = -10.0;
	const double goalPos = 20;
	double intermediatePositions[10];

	double pResult[2] = {0, 0};
	double pCurrentPosition[2] = { initialPos, initialPos};
	double pDesiredPosition[2] = { goalPos, goalPos };
	double pMaxVelocity[1] = { 2000.0 };
	double pCycleTime[1] = { 5.0 };
	double pDurationInCycles[1] = { 5.0 };
	double pCurrentCycleNumber[1] = { 1.0 };

	double *ppParameters[7] = {
		pResult,
		pCurrentPosition,
		pDesiredPosition,
		pMaxVelocity,
		pCycleTime,
		pDurationInCycles,
		pCurrentCycleNumber};

	// Test some intermediate steps in the interpolation
	KukaLBR4OperationsNS::SimpleInterpolation(ppParameters, 2);

	intermediatePositions[0] = pResult[0];

	pCurrentCycleNumber[0] = 2;

	KukaLBR4OperationsNS::SimpleInterpolation(ppParameters, 2);

	intermediatePositions[1] = pResult[0];

	KUKALBR4NATIVEASSERT(abs((intermediatePositions[0] - initialPos) - (intermediatePositions[1] - intermediatePositions[0])) < 0.000005, "Interpolation is smooth");

	// Test the final step of the interpolation
	pCurrentCycleNumber[0] = 5;

	KukaLBR4OperationsNS::SimpleInterpolation(ppParameters, 2);

	KUKALBR4NATIVEASSERT((abs(pResult[0] - goalPos) < ERROR_EPSILON), "First joint reached goal.");
	KUKALBR4NATIVEASSERT((abs(pResult[1] - goalPos) < ERROR_EPSILON), "Second joint reached goal.");
}

/// <summary> Test the <c>CeeOpAddVec7</c> operation </summary>
void KukaLBR4OperationsTest::TestCeeOpAddVec7()
{
	// srand cannot take a value larger than an unsigned int, so no reason to use GetTickCount64()
#pragma warning(suppress: 28159)
	srand(GetTickCount());

	double pOperand1[7] = {
		KukaLBR4OperationsTest::GetRandomNumber(),
		KukaLBR4OperationsTest::GetRandomNumber(),
		KukaLBR4OperationsTest::GetRandomNumber(),
		KukaLBR4OperationsTest::GetRandomNumber(),
		KukaLBR4OperationsTest::GetRandomNumber(),
		KukaLBR4OperationsTest::GetRandomNumber(),
		KukaLBR4OperationsTest::GetRandomNumber() };

	double pOperand2[7] = {
		KukaLBR4OperationsTest::GetRandomNumber(),
		KukaLBR4OperationsTest::GetRandomNumber(),
		KukaLBR4OperationsTest::GetRandomNumber(),
		KukaLBR4OperationsTest::GetRandomNumber(),
		KukaLBR4OperationsTest::GetRandomNumber(),
		KukaLBR4OperationsTest::GetRandomNumber(),
		KukaLBR4OperationsTest::GetRandomNumber() };

	double pResult[7];

	double *ppParameters[5] = {
		pResult,
		pOperand1,
		pOperand2 };

	KukaLBR4OperationsNS::CeeOpAddVec7(ppParameters);

	for (int i = 0; i < 7; ++i)
	{
		KUKALBR4NATIVEASSERT(pResult[i] == pOperand1[i] + pOperand2[i], "Each element in result is the sum of the corresponding elements in the two operands.");
	}
}

/// <summary> Test the <c>CeeOpSubtractVec7</c> operation </summary>
void KukaLBR4OperationsTest::TestCeeOpSubtractVec7()
{
	// srand cannot take a value larger than an unsigned int, so no reason to use GetTickCount64()
#pragma warning(suppress: 28159)
	srand(GetTickCount());

	double pOperand1[7] = {
		KukaLBR4OperationsTest::GetRandomNumber(),
		KukaLBR4OperationsTest::GetRandomNumber(),
		KukaLBR4OperationsTest::GetRandomNumber(),
		KukaLBR4OperationsTest::GetRandomNumber(),
		KukaLBR4OperationsTest::GetRandomNumber(),
		KukaLBR4OperationsTest::GetRandomNumber(),
		KukaLBR4OperationsTest::GetRandomNumber() };

	double pOperand2[7] = {
		KukaLBR4OperationsTest::GetRandomNumber(),
		KukaLBR4OperationsTest::GetRandomNumber(),
		KukaLBR4OperationsTest::GetRandomNumber(),
		KukaLBR4OperationsTest::GetRandomNumber(),
		KukaLBR4OperationsTest::GetRandomNumber(),
		KukaLBR4OperationsTest::GetRandomNumber(),
		KukaLBR4OperationsTest::GetRandomNumber() };

	double pResult[7];

	double *ppParameters[5] = {
		pResult,
		pOperand1,
		pOperand2 };

	KukaLBR4OperationsNS::CeeOpSubtractVec7(ppParameters);

	for (int i = 0; i < 7; ++i)
	{
		KUKALBR4NATIVEASSERT(pResult[i] == pOperand1[i] - pOperand2[i], "Each element in result is the sum of the corresponding elements in the two operands.");
	}
}

/// <summary> Test the <c>CeeOpAreScalarsEqual</c> operation </summary>
void KukaLBR4OperationsTest::TestCeeOpAreScalarsEqual()
{
	// srand cannot take a value larger than an unsigned int, so no reason to use GetTickCount64()
#pragma warning(suppress: 28159)
	srand(GetTickCount());

	double pOperand1[1] = { 15 };
	double pOperand2[1] = { 15 };
	double pResult[1];

	double *ppParameters[3] = {
		pResult,
		pOperand1,
		pOperand2 };

	KukaLBR4OperationsNS::CeeOpAreScalarsEqual(ppParameters);

	KUKALBR4NATIVEASSERT(pResult[0] == 1, "Scalars are correctly indicated as equal");

	pOperand2[0] = 16;

	KukaLBR4OperationsNS::CeeOpAreScalarsEqual(ppParameters);

	KUKALBR4NATIVEASSERT(pResult[0] == 0, "Scalars are correctly indicated as not equal");
}

/// <summary> Test the <c>CeeOpIsForceMagnitudeGreaterThanScalar</c> operation. </summary>
void KukaLBR4OperationsTest::TestCeeOpIsForceMagnitudeGreaterThanScalar()
{
	// Test true case first
	double pOperand1[6] =
	{
		1.0,
		2.0,
		2.0,
		0.0,
		0.0,
		0.0
	};
	double pOperand2[1] = { 2.9 };
	double pResult[1];

	double *ppParameters[3] =
	{
		pResult,
		pOperand1,
		pOperand2
	};

	KukaLBR4OperationsNS::CeeOpIsForceMagnitudeGreaterThanScalar(ppParameters);

	KUKALBR4NATIVEASSERT(pResult[0] == 1, "Check that result is one");

	pOperand1[1] = 0;

	KukaLBR4OperationsNS::CeeOpIsForceMagnitudeGreaterThanScalar(ppParameters);

	KUKALBR4NATIVEASSERT(pResult[0] == 0, "Check that result is zero");
}

/// <summary> Test the <c>CeeOpMulitplyFrames</c> operation. </summary>
void KukaLBR4OperationsTest::TestMultiplyFrames()
{
	double pA[CARTESIAN_FRAME_LENGTH] = { 1.0, 0.0, 0.0, 1.0, 0.0, 0.0, 1.0, 2.0, 0.0, 1.0, 0.0, 3.0 };
	double pB[CARTESIAN_FRAME_LENGTH] = { 0.0, 1.0, 0.0, 4.0, 0.0, 0.0, 1.0, 5.0, 1.0, 0.0, 0.0, 6.0 };
	double pExpectedResult[CARTESIAN_FRAME_LENGTH] = {0.0, 1.0, 0.0, 5.0, 1.0, 0.0, 0.0, 8.0, 0.0, 0.0, 1.0, 8.0};
	double pResult[CARTESIAN_FRAME_LENGTH];

	double *ppParams[3] = { pResult, pA, pB };

	KukaLBR4OperationsNS::CeeOpMultiplyFrames(ppParams);

	for(int i = 0; i < CARTESIAN_FRAME_LENGTH; ++i)
	{
		KUKALBR4NATIVEASSERT(pResult[i] == pExpectedResult[i], "Confirm elements of expected and actual result match");
	}
}

/// <summary> Returns a random number </summary>
/// <returns> A random value between 0.0 and MaxRandValue </returns>
double KukaLBR4OperationsTest::GetRandomNumber()
{
	return rand() % KukaLBR4OperationsTest::MaxRandValue;
}

/// <summary> Get a random unit quaternion </summary>
/// <param name="pQuat"> Point to location to store random unit quaternion in. </param>;
void KukaLBR4OperationsTest::GetRandomUnitQuaternion(double *pQuat)
{
	double sumSquare = 0;

	for(int i = 0; i < QUATERNION_LENGTH; ++i)
	{
		pQuat[i] = ((double)rand()/(double)RAND_MAX);
		sumSquare += pQuat[i] * pQuat[i];
	}

	double norm = sqrt(sumSquare);

	for(int i = 0; i < QUATERNION_LENGTH; ++i)
	{
		pQuat[i] /= norm;
	}
}

/// <summary> Test the conversions between quaternions and frames. </summary>
void KukaLBR4OperationsTest::TestQuaternionFrameConversions()
{
	double pQuat[QUATERNION_LENGTH] = {0, 0, 0, 0};

	GetRandomUnitQuaternion(pQuat);
	TestQuaternionFrameConversions(pQuat);

	pQuat[0] = 0.0001;
	pQuat[1] = 0.8;
	pQuat[2] = 0.55;
	double rem = sqrt(1.0 - ((pQuat[0] * pQuat[0]) + (pQuat[1] * pQuat[1]) + (pQuat[2] * pQuat[2])));
	pQuat[3] = rem;
	TestQuaternionFrameConversions(pQuat);

	pQuat[2] = 0.8;
	pQuat[1] = 0.55;
	TestQuaternionFrameConversions(pQuat);

	pQuat[3] = 0.8;
	pQuat[2] = rem;
	TestQuaternionFrameConversions(pQuat);
}

/// <summary> Run a single test of conversions between quaternions and frames. </summary>
void KukaLBR4OperationsTest::TestQuaternionFrameConversions(double pQuat[4])
{
	double pQuatConverted[QUATERNION_LENGTH] = {0, 0, 0, 0};
	double pFrame[CARTESIAN_FRAME_LENGTH] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};

	KukaLBR4OperationsNS::QuaternionToFrame(pQuat, pFrame);
	KukaLBR4OperationsNS::FrameToQuaternion(pFrame, pQuatConverted);

	for(int i = 0; i < QUATERNION_LENGTH; ++i)
	{
		KUKALBR4NATIVEASSERT( abs(pQuat[i] - pQuatConverted[i]) < ERROR_EPSILON || abs(pQuat[i] + pQuatConverted[i]) < ERROR_EPSILON,
			"Test that quaternion elements are same after conversion to frame and back");
	}
}
/// <summary> Test the copy frame operation. </summary>
void KukaLBR4OperationsTest::TestCeeOpCopyFrame()
{
	// srand cannot take a value larger than an unsigned int, so no reason to use GetTickCount64()
#pragma warning(suppress: 28159)
	srand(GetTickCount());

	double pDestination[CARTESIAN_FRAME_LENGTH];
	double pSource[CARTESIAN_FRAME_LENGTH];

	for (int i = 0; i < CARTESIAN_FRAME_LENGTH; ++i)
	{
		pSource[i] = KukaLBR4OperationsTest::GetRandomNumber();
	}

	double *ppParameters[2] = 
	{
		pDestination,
		pSource
	};

	KukaLBR4OperationsNS::CeeOpCopyFrame(ppParameters);

	for (int i = 0; i < CARTESIAN_FRAME_LENGTH; ++i)
	{
		KUKALBR4NATIVEASSERT(pDestination[i] == pSource[i], "Source and destination values match");
	}
}

/// <summary> Test projection of a vector to a plane. </summary>
void KukaLBR4OperationsTest::TestVector3PlaneProject()
{
	// srand cannot take a value larger than an unsigned int, so no reason to use GetTickCount64()
#pragma warning(suppress: 28159)
	srand(GetTickCount());

	double pRandomVector[3] = 
	{
		GetRandomNumber(),
		GetRandomNumber(),
		GetRandomNumber()
	};

	double pResult[3];

	KukaLBR4OperationsNS::Vector3PlaneProject(pRandomVector, pRandomVector, pResult);

	for (int i = 0; i < 3; ++i)
	{
		KUKALBR4NATIVEASSERT(
			abs(pResult[i]) < ERROR_EPSILON,
			"Test that a vector orthogonal to the plane results in a vector of zeros when projected into the plane.");
	}

	// unit vector in the +X direction
	double pXVector[3] = { 1, 0, 0 };
	
	// unit vector in the +YZ direction
	double pYZVector[3] = { 0, sin(M_PI_4), sin(M_PI_4) };

	KukaLBR4OperationsNS::Vector3PlaneProject(pYZVector, pXVector, pResult);

	for (int i = 0; i < 3; ++i)
	{
		KUKALBR4NATIVEASSERT(
			abs(pResult[i]-pYZVector[i]) < ERROR_EPSILON,
			"Test that a vector in the plane results in itself when projected into the plane.");
	}

	double pXZVector[3] = { sin(M_PI_4), 0, sin(M_PI_4) };

	KukaLBR4OperationsNS::Vector3PlaneProject(pXZVector, pXVector, pResult);

		KUKALBR4NATIVEASSERT(
			abs(pResult[0] - 0) < ERROR_EPSILON,
			"Test that a vector in the XZ direction results in a vector with only a Z component when projected into the YZ plane.");

		KUKALBR4NATIVEASSERT(
			abs(pResult[1] - 0) < ERROR_EPSILON,
			"Test that a vector in the XZ direction results in a vector with only a Z component when projected into the YZ plane.");

		KUKALBR4NATIVEASSERT(
			abs(pResult[2] - pXZVector[2]) < ERROR_EPSILON,
			"Test that a vector in the XZ direction results in a vector with only a Z component when projected into the YZ plane.");
}

/// <summary> Test projection of a 3-dim vector onto another vector. </summary>
void KukaLBR4OperationsTest::TestVector3Vector3Project()
{
	// srand cannot take a value larger than an unsigned int, so no reason to use GetTickCount64()
#pragma warning(suppress: 28159)
	srand(GetTickCount());

	double pRandomVector[3] = 
	{
		GetRandomNumber(),
		GetRandomNumber(),
		GetRandomNumber()
	};

	double randomVectorLength = sqrt(
		pRandomVector[0] * pRandomVector[0] +
		pRandomVector[1] * pRandomVector[1] +
		pRandomVector[2] * pRandomVector[2]); 

	double pUnitVector[3] =
	{
		pRandomVector[0] / randomVectorLength,
		pRandomVector[1] / randomVectorLength,
		pRandomVector[2] / randomVectorLength
	};

	double pResult[3];

	KukaLBR4OperationsNS::Vector3Vector3Project(pRandomVector, pUnitVector, pResult);

	for (int i = 0; i < 3; ++i)
	{
		KUKALBR4NATIVEASSERT(
			abs(pResult[i] - pRandomVector[i]) < ERROR_EPSILON,
			"Test that a vector projected onto a unit vector in the same direction is itself.");
	}

	// unit vector in the +X direction
	double pXVector[3] = { 1, 0, 0 };
	
	// unit vector in the +YZ direction
	double pYZVector[3] = { 0, sin(M_PI_4), sin(M_PI_4) };

	KukaLBR4OperationsNS::Vector3Vector3Project(pYZVector, pXVector, pResult);

	for (int i = 0; i < 3; ++i)
	{
		KUKALBR4NATIVEASSERT(
			abs(pResult[i] - 0) < ERROR_EPSILON,
			"Test that a vector projected onto an orthogonal vector results in a vector of all zeros.");
	}

	double pXZVector[3] = { sin(M_PI_4), 0, sin(M_PI_4) };

	KukaLBR4OperationsNS::Vector3Vector3Project(pXZVector, pXVector, pResult);

	KUKALBR4NATIVEASSERT(
		abs(pResult[0] - pXZVector[0]) < ERROR_EPSILON,
		"Test that a vector in the XZ direction, projected onto [1, 0, 0] retains only it's X component.");

	KUKALBR4NATIVEASSERT(
		abs(pResult[1] - 0) < ERROR_EPSILON,
		"Test that a vector in the XZ direction, projected onto [1, 0, 0] retains only it's X component.");

	KUKALBR4NATIVEASSERT(
		abs(pResult[2] - 0) < ERROR_EPSILON,
		"Test that a vector in the XZ direction, projected onto [1, 0, 0] retains only it's X component.");
}

/// <summary> Test computing the magnitude of a vector .
void KukaLBR4OperationsTest::TestVectorMagnitude()
{
	// srand cannot take a value larger than an unsigned int, so no reason to use GetTickCount64()
#pragma warning(suppress: 28159)
	srand(GetTickCount());

	double pRandomVector[3] = 
	{
		GetRandomNumber(),
		GetRandomNumber(),
		GetRandomNumber()
	};

	double result;

	KukaLBR4OperationsNS::VectorMagnitude(pRandomVector, 3, &result);

	for(int i = 0; i < 3; ++i)
	{
		pRandomVector[i] = pRandomVector[i] / result;
	}

	KukaLBR4OperationsNS::VectorMagnitude(pRandomVector, 3, &result);
	std::stringstream errorMessageStream;
	errorMessageStream << "Test Mag(randVec/Mag(randVec)) of [" << pRandomVector[0] << "," << pRandomVector[1] << "," << pRandomVector[2] << "] equals 1";
	KUKALBR4NATIVEASSERT(abs(result - 1) < ERROR_EPSILON, errorMessageStream.str());
}

/// <summary> Test comparison of magnitude of a force projected into a plane to a scalar value. </summary>
void KukaLBR4OperationsTest::TestCeeOpIsForceInPlaneGreaterThanScalar()
{
	double pXVector[3] = { 1, 0, 0 };
	double pYVector[3] = { 0, 1, 0 };
	double pZVector[3] = { 0, 0, 1 };

	double p2NX[3] = { 2, 0, 0 };
	double p2NY[3] = { 0, 2, 0 };
	double p2NZ[3] = { 0, 0, 2 };

	double result;
	double threshold;

	double **ppParameters = new double*[4];

	// check that a force in the same direction as the vector orthogonal to the plane does not 
	// generate a positive result
	ppParameters[0] = &result;
	ppParameters[1] = p2NX;
	ppParameters[2] = pXVector;
	ppParameters[3] = &threshold;

	threshold = 1.5;

	KukaLBR4OperationsNS::CeeIsForceInPlaneGreaterThanScalar(ppParameters);
	KUKALBR4NATIVEASSERT(result == 0, "X vector is not projected into YZ plane");

	ppParameters[1] = p2NY;
	ppParameters[2] = pYVector;
	KukaLBR4OperationsNS::CeeIsForceInPlaneGreaterThanScalar(ppParameters);
	KUKALBR4NATIVEASSERT(result == 0, "Y vector is not projected into XZ plane");

	ppParameters[1] = p2NZ;
	ppParameters[2] = pZVector;
	KukaLBR4OperationsNS::CeeIsForceInPlaneGreaterThanScalar(ppParameters);
	KUKALBR4NATIVEASSERT(result == 0, "Z vector is not projected into XY plane");

	// check that a force above the threshold inside the plane does generate a positive result
	ppParameters[1] = p2NX;
	ppParameters[2] = pYVector;
	KukaLBR4OperationsNS::CeeIsForceInPlaneGreaterThanScalar(ppParameters);
	KUKALBR4NATIVEASSERT(result == 1, "X vector is projected into XZ plane");

	ppParameters[1] = p2NY;
	ppParameters[2] = pXVector;
	KukaLBR4OperationsNS::CeeIsForceInPlaneGreaterThanScalar(ppParameters);
	KUKALBR4NATIVEASSERT(result == 1, "Y vector is projected into YZ plane");

	ppParameters[1] = p2NZ;
	ppParameters[2] = pYVector;
	KukaLBR4OperationsNS::CeeIsForceInPlaneGreaterThanScalar(ppParameters);
	KUKALBR4NATIVEASSERT(result == 1, "Z vector is projected into XZ plane");

	// check that a force below the threshold inside the plane does not generate a positive result
	threshold = 2.1;

	ppParameters[1] = p2NX;
	ppParameters[2] = pYVector;
	KukaLBR4OperationsNS::CeeIsForceInPlaneGreaterThanScalar(ppParameters);
	KUKALBR4NATIVEASSERT(result == 0, "Threshold is computed correctly for force in X projected into XZ plane");

	ppParameters[1] = p2NY;
	ppParameters[2] = pXVector;
	KukaLBR4OperationsNS::CeeIsForceInPlaneGreaterThanScalar(ppParameters);
	KUKALBR4NATIVEASSERT(result == 0, "Threshold is computed correctly for force in Y projected into YZ plane");

	ppParameters[1] = p2NZ;
	ppParameters[2] = pYVector;
	KukaLBR4OperationsNS::CeeIsForceInPlaneGreaterThanScalar(ppParameters);
	KUKALBR4NATIVEASSERT(result == 0, "Threshold is computed correctly for force in Z projected into XZ plane");
}

/// <summary> Test comparison of magnitude of a force rejected from a plane to a scalar value. </summary>
void KukaLBR4OperationsTest::TestCeeOpIsForceOrthogonalToPlaneGreaterThanScalar()
{
	double pXVector[3] = { 1, 0, 0 };
	double pYVector[3] = { 0, 1, 0 };
	double pZVector[3] = { 0, 0, 1 };

	double p2NX[3] = { 2, 0, 0 };
	double p2NY[3] = { 0, 2, 0 };
	double p2NZ[3] = { 0, 0, 2 };

	double result;
	double threshold;

	double **ppParameters = new double*[4];

	// check that a force in the same direction as the vector orthogonal to the plane does 
	// generate a positive result
	ppParameters[0] = &result;
	ppParameters[1] = p2NX;
	ppParameters[2] = pXVector;
	ppParameters[3] = &threshold;

	threshold = 1.5;

	KukaLBR4OperationsNS::CeeIsForceOrthogonalToPlaneGreaterThanScalar(ppParameters);
	KUKALBR4NATIVEASSERT(result == 1, "X vector is rejected from YZ plane");

	ppParameters[1] = p2NY;
	ppParameters[2] = pYVector;
	KukaLBR4OperationsNS::CeeIsForceOrthogonalToPlaneGreaterThanScalar(ppParameters);
	KUKALBR4NATIVEASSERT(result == 1, "Y vector is rejected from XZ plane");

	ppParameters[1] = p2NZ;
	ppParameters[2] = pZVector;
	KukaLBR4OperationsNS::CeeIsForceOrthogonalToPlaneGreaterThanScalar(ppParameters);
	KUKALBR4NATIVEASSERT(result == 1, "Z vector is rejected from XY plane");

	// check that a force in the plane does not genreate a positive result
	ppParameters[1] = p2NX;
	ppParameters[2] = pZVector;
	KukaLBR4OperationsNS::CeeIsForceOrthogonalToPlaneGreaterThanScalar(ppParameters);
	KUKALBR4NATIVEASSERT(result == 0, "X vector is not rejected from XY plane");

	ppParameters[1] = p2NY;
	ppParameters[2] = pXVector;
	KukaLBR4OperationsNS::CeeIsForceOrthogonalToPlaneGreaterThanScalar(ppParameters);
	KUKALBR4NATIVEASSERT(result == 0, "Y vector is not rejected from YZ plane");

	ppParameters[1] = p2NZ;
	ppParameters[2] = pYVector;
	KukaLBR4OperationsNS::CeeIsForceOrthogonalToPlaneGreaterThanScalar(ppParameters);
	KUKALBR4NATIVEASSERT(result == 0, "Z vector is not rejected from XZ plane");

	// check that a force orthogonal to the plane that is below the threshold does not generate a positive
	// result
	threshold = 2.1;

	ppParameters[1] = p2NX;
	ppParameters[2] = pXVector;
	KukaLBR4OperationsNS::CeeIsForceOrthogonalToPlaneGreaterThanScalar(ppParameters);
	KUKALBR4NATIVEASSERT(result == 0, "Threshold works for forces in X direction");

	ppParameters[1] = p2NY;
	ppParameters[2] = pYVector;
	KukaLBR4OperationsNS::CeeIsForceOrthogonalToPlaneGreaterThanScalar(ppParameters);
	KUKALBR4NATIVEASSERT(result == 0, "Threshold works for forces in Y direction");

	ppParameters[1] = p2NZ;
	ppParameters[2] = pZVector;
	KukaLBR4OperationsNS::CeeIsForceOrthogonalToPlaneGreaterThanScalar(ppParameters);
	KUKALBR4NATIVEASSERT(result == 0, "Threshold works for forces in Z direction");
}

/// <summary> Test the <c>CeeOpAdd</c> operation </summary>
/// <param name="resultString"> buffer for a failure message or indication that test passed. </param>
/// <param name="buffersize"> length of <c>resultString</c> buffer. </param>
void TestCeeOpAdd(char *resultString, int bufferSize)
{
	try
	{
		KukaLBR4OperationsTest::TestCeeOpAdd();
	}
	catch (const char *ex)
	{
		strcpy_s(resultString, bufferSize, ex);
		return;
	}

	strcpy_s(resultString, bufferSize, KukaLBR4NativeTests::pPassString);
}

/// <summary> Test the <c>CeeOpAddVec6</c> operation </summary>
/// <param name="resultString"> buffer for a failure message or indication that test passed. </param>
/// <param name="buffersize"> length of <c>resultString</c> buffer. </param>
void TestCeeOpAddVec6(char *resultString, int bufferSize)
{
	try
	{
		KukaLBR4OperationsTest::TestCeeOpAddVec6();
	}
	catch (const char *ex)
	{
		strcpy_s(resultString, bufferSize, ex);
		return;
	}

	strcpy_s(resultString, bufferSize, KukaLBR4NativeTests::pPassString);
}

/// <summary> Test the <c>CeeOpMultiply</c> operation </summary>
/// <param name="resultString"> buffer for a failure message or indication that test passed. </param>
/// <param name="buffersize"> length of <c>resultString</c> buffer. </param>
void TestCeeOpMultiply(char *resultString, int bufferSize)
{
	try
	{
		KukaLBR4OperationsTest::TestCeeOpMultiply();
	}
	catch (const char *ex)
	{
		strcpy_s(resultString, bufferSize, ex);
		return;
	}

	strcpy_s(resultString, bufferSize, KukaLBR4NativeTests::pPassString);
}

/// <summary> Test the <c>CeeOpIsMagnitudeGreaterThanVec6</c> operation </summary>
/// <param name="resultString"> buffer for a failure message or indication that test passed. </param>
/// <param name="buffersize"> length of <c>resultString</c> buffer. </param>
void TestCeeOpIsMagnitudeGreaterThanVec6(char *resultString, int bufferSize)
{
	try
	{
		KukaLBR4OperationsTest::TestCeeOpIsMagnitudeGreaterThanVec6();
	}
	catch (const char *ex)
	{
		strcpy_s(resultString, bufferSize, ex);
		return;
	}

	strcpy_s(resultString, bufferSize, KukaLBR4NativeTests::pPassString);
}

/// <summary> Test the <c>CeeOpIsAnyElementGreaterThanVec6</c> operation </summary>
/// <param name="resultString"> buffer for a failure message or indication that test passed. </param>
/// <param name="buffersize"> length of <c>resultString</c> buffer. </param>
void TestCeeOpIsAnyElementGreaterThanVec6(char *resultString, int bufferSize)
{
	try
	{
		KukaLBR4OperationsTest::TestCeeOpIsAnyElementGreaterThanVec6();
	}
	catch (const char *ex)
	{
		strcpy_s(resultString, bufferSize, ex);
		return;
	}

	strcpy_s(resultString, bufferSize, KukaLBR4NativeTests::pPassString);
}

/// <summary> Test the <c>CeeOpCopyVec7</c> operation </summary>
/// <param name="resultString"> buffer for a failure message or indication that test passed. </param>
/// <param name="buffersize"> length of <c>resultString</c> buffer. </param>
void TestCeeOpCopyVec7(char *resultString, int bufferSize)
{
	try
	{
		KukaLBR4OperationsTest::TestCeeOpCopyVec7();
	}
	catch (const char *ex)
	{
		strcpy_s(resultString, bufferSize, ex);
		return;
	}

	strcpy_s(resultString, bufferSize, KukaLBR4NativeTests::pPassString);
}

/// <summary> Test the <c>CeeOpCopyVec6</c> operation </summary>
/// <param name="resultString"> buffer for a failure message or indication that test passed. </param>
/// <param name="buffersize"> length of <c>resultString</c> buffer. </param>
void TestCeeOpCopyVec6(char *resultString, int bufferSize)
{
	try
	{
		KukaLBR4OperationsTest::TestCeeOpCopyVec6();
	}
	catch (const char *ex)
	{
		strcpy_s(resultString, bufferSize, ex);
		return;
	}

	strcpy_s(resultString, bufferSize, KukaLBR4NativeTests::pPassString);
}

/// <summary> Test the <c>CeeOpSimpleJointPositionInterpolation</c> operation </summary>
/// <param name="resultString"> buffer for a failure message or indication that test passed. </param>
/// <param name="buffersize"> length of <c>resultString</c> buffer. </param>
void TestCeeOpSimpleJointPositionInterpolation(char *resultString, int bufferSize)
{
	try
	{
		KukaLBR4OperationsTest::TestCeeOpSimpleJointInterpolation();
	}
	catch (const char *ex)
	{
		strcpy_s(resultString, bufferSize, ex);
		return;
	}

	strcpy_s(resultString, bufferSize, KukaLBR4NativeTests::pPassString);
}

/// <summary> Test the <c>CeeOpAddVec7</c> operation </summary>
/// <param name="resultString"> buffer for a failure message or indication that test passed. </param>
/// <param name="buffersize"> length of <c>resultString</c> buffer. </param>
void TestCeeOpAddVec7(char *resultString, int bufferSize)
{
	try
	{
		KukaLBR4OperationsTest::TestCeeOpAddVec7();
	}
	catch (const char *ex)
	{
		strcpy_s(resultString, bufferSize, ex);
		return;
	}

	strcpy_s(resultString, bufferSize, KukaLBR4NativeTests::pPassString);
}

/// <summary> Test the <c>CeeOpSubtractVec7</c> operation </summary>
/// <param name="resultString"> buffer for a failure message or indication that test passed. </param>
/// <param name="buffersize"> length of <c>resultString</c> buffer. </param>
void TestCeeOpSubtractVec7(char *resultString, int bufferSize)
{
	try
	{
		KukaLBR4OperationsTest::TestCeeOpSubtractVec7();
	}
	catch (const char *ex)
	{
		strcpy_s(resultString, bufferSize, ex);
		return;
	}

	strcpy_s(resultString, bufferSize, KukaLBR4NativeTests::pPassString);
}

/// <summary> Test the <c>CeeOpAreScalarsEqual</c> operation </summary>
/// <param name="resultString"> buffer for a failure message or indication that test passed. </param>
/// <param name="buffersize"> length of <c>resultString</c> buffer. </param>
void TestCeeOpAreScalarsEqual(char *resultString, int bufferSize)
{
	try
	{
		KukaLBR4OperationsTest::TestCeeOpAreScalarsEqual();
	}
	catch (const char *ex)
	{
		strcpy_s(resultString, bufferSize, ex);
		return;
	}

	strcpy_s(resultString, bufferSize, KukaLBR4NativeTests::pPassString);
}

/// <summary> Test the <c>CeeOpSimpleCartesianInterpolation</c> operation </summary>
/// <param name="resultString"> buffer for a failure message or indication that test passed. </param>
/// <param name="buffersize"> length of <c>resultString</c> buffer. </param>
void TestCeeOpSimpleCartesianInterpolation(char *resultString, int bufferSize)
{
	try
	{
		KukaLBR4OperationsTest::TestCeeOpSimpleCartesianInterpolation();
	}
	catch (const char *ex)
	{
		strcpy_s(resultString, bufferSize, ex);
		return;
	}

	strcpy_s(resultString, bufferSize, KukaLBR4NativeTests::pPassString);
}

/// <summary> Test the <c>CeeOpSimpleCartesianInterpolation</c> operation </summary>
/// <param name="resultString"> buffer for a failure message or indication that test passed. </param>
/// <param name="buffersize"> length of <c>resultString</c> buffer. </param>
void TestCeeOpSimpleCartesianInterpolation2(char *resultString, int bufferSize)
{
	try
	{
		KukaLBR4OperationsTest::TestCeeOpSimpleCartesianInterpolation2();
	}
	catch (const char *ex)
	{
		strcpy_s(resultString, bufferSize, ex);
		return;
	}

	strcpy_s(resultString, bufferSize, KukaLBR4NativeTests::pPassString);
}

/// <summary> Test the <c>SimpleInterpolation</c> operation </summary>
/// <param name="resultString"> buffer for a failure message or indication that test passed. </param>
/// <param name="buffersize"> length of <c>resultString</c> buffer. </param>
void TestSimpleInterpolation(char *resultString, int bufferSize)
{
	try
	{
		KukaLBR4OperationsTest::TestSimpleInterpolation();
	}
	catch (const char *ex)
	{
		strcpy_s(resultString, bufferSize, ex);
		return;
	}

	strcpy_s(resultString, bufferSize, KukaLBR4NativeTests::pPassString);
}

/// <summary> Test conversion between frame and quaternions </summary>
/// <param name="resultString"> buffer for a failure message or indication that test passed. </param>
/// <param name="buffersize"> length of <c>resultString</c> buffer. </param>
void TestQuaternionFrameConversions(char *resultString, int bufferSize)
{
	try
	{
		KukaLBR4OperationsTest::TestQuaternionFrameConversions();
	}
	catch (const char *ex)
	{
		strcpy_s(resultString, bufferSize, ex);
		return;
	}

	strcpy_s(resultString, bufferSize, KukaLBR4NativeTests::pPassString);
}

/// <summary> Test the multiplication of frames </summary>
/// <param name="resultString"> buffer for a failure message or indication that test passed. </param>
/// <param name="buffersize"> length of <c>resultString</c> buffer. </param>
void TestMultiplyFrames(char *resultString, int bufferSize)
{
	try
	{
		KukaLBR4OperationsTest::TestMultiplyFrames();
	}
	catch (const char *ex)
	{
		strcpy_s(resultString, bufferSize, ex);
		return;
	}

	strcpy_s(resultString, bufferSize, KukaLBR4NativeTests::pPassString);
}

/// <summary> Test copying a frame </summary>.
/// <param name="resultString"> buffer for a failure message or indication that test passed. </param>
/// <param name="buffersize"> length of <c>resultString</c> buffer. </param>
void TestCeeOpCopyFrame(char *resultString, int bufferSize)
{
	try
	{
		KukaLBR4OperationsTest::TestCeeOpCopyFrame();
	}
	catch (const char *ex)
	{
		strcpy_s(resultString, bufferSize, ex);
		return;
	}

	strcpy_s(resultString, bufferSize, KukaLBR4NativeTests::pPassString);
}

/// <summary> Test the <c>CeeOpIsForceMagnitudeGreaterThanScalar</c> operation. </summary>
/// <param name="resultString"> buffer for a failure message or indication that test passed. </param>
/// <param name="buffersize"> length of <c>resultString</c> buffer. </param>
void TestCeeOpIsForceMagnitudeGreaterThanScalar(char *resultString, int bufferSize)
{
	try
	{
		KukaLBR4OperationsTest::TestCeeOpIsForceMagnitudeGreaterThanScalar();
	}
	catch (const char *ex)
	{
		strcpy_s(resultString, bufferSize, ex);
		return;
	}

	strcpy_s(resultString, bufferSize, KukaLBR4NativeTests::pPassString);
}

/// <summary> Test projection of a vector to a plane. </summary>
/// <param name="resultString"> buffer for a failure message or indication that test passed. </param>
/// <param name="buffersize"> length of <c>resultString</c> buffer. </param>
void TestVector3PlaneProject(char *resultString, int bufferSize)
{
	try
	{
		KukaLBR4OperationsTest::TestVector3PlaneProject();
	}
	catch (const char *ex)
	{
		strcpy_s(resultString, bufferSize, ex);
		return;
	}

	strcpy_s(resultString, bufferSize, KukaLBR4NativeTests::pPassString);
}

/// <summary> Test projection of a 3-dim vector onto another vector. </summary>
/// <param name="resultString"> buffer for a failure message or indication that test passed. </param>
/// <param name="buffersize"> length of <c>resultString</c> buffer. </param>
void TestVector3Vector3Project(char *resultString, int bufferSize)
{
	try
	{
		KukaLBR4OperationsTest::TestVector3Vector3Project();
	}
	catch (const char *ex)
	{
		strcpy_s(resultString, bufferSize, ex);
		return;
	}

	strcpy_s(resultString, bufferSize, KukaLBR4NativeTests::pPassString);
}

/// <summary> Test computing the magnitude of a vector .
/// <param name="resultString"> buffer for a failure message or indication that test passed. </param>
/// <param name="buffersize"> length of <c>resultString</c> buffer. </param>
void TestVectorMagnitude(char *resultString, int bufferSize)
{
	try
	{
		KukaLBR4OperationsTest::TestVectorMagnitude();
	}
	catch (const char *ex)
	{
		strcpy_s(resultString, bufferSize, ex);
		return;
	}

	strcpy_s(resultString, bufferSize, KukaLBR4NativeTests::pPassString);
}

/// <summary> Test comparison of magnitude of a force projected into a plane to a scalar value. </summary>
/// <param name="resultString"> buffer for a failure message or indication that test passed. </param>
/// <param name="buffersize"> length of <c>resultString</c> buffer. </param>
void TestCeeOpIsForceInPlaneGreaterThanScalar(char *resultString, int bufferSize)
{
	try
	{
		KukaLBR4OperationsTest::TestCeeOpIsForceInPlaneGreaterThanScalar();
	}
	catch (const char *ex)
	{
		strcpy_s(resultString, bufferSize, ex);
		return;
	}

	strcpy_s(resultString, bufferSize, KukaLBR4NativeTests::pPassString);
}

/// <summary> Test comparison of magnitude of a force rejected from a plane to a scalar value. </summary>
/// <param name="resultString"> buffer for a failure message or indication that test passed. </param>
/// <param name="buffersize"> length of <c>resultString</c> buffer. </param>
void TestCeeOpIsForceOrthogonalToPlaneGreaterThanScalar(
	char *resultString,
	int bufferSize)
{
	try
	{
		KukaLBR4OperationsTest::TestCeeOpIsForceOrthogonalToPlaneGreaterThanScalar();
	}
	catch (const char *ex)
	{
		strcpy_s(resultString, bufferSize, ex);
		return;
	}

	strcpy_s(resultString, bufferSize, KukaLBR4NativeTests::pPassString);
}
